using System;
using System.Collections.Generic;
using Microsoft.Xna.Framework;
using SpaceGame.Util;

namespace SpaceGame.Gameplay.AI.Pathfinding
{

    /// <summary>
    /// Base class for pathfinding nodes, it holds no actual information about the map. 
    /// </summary>
    class AStarNode : IComparable
    {
        #region Private fields
        /// <summary>
        /// The parent of the node.
        /// </summary>
        private AStar controller;
        #endregion

        #region Properties
        /// <summary>
        /// The mesh node associated with this node in the pathfinding algorithm
        /// </summary>
        public MeshNode MeshNode
        {
            get;
            private set;
        }
        /// <summary>
        /// The parent of the node.
        /// </summary>
        public AStarNode Parent
        {
            get;
            private set;

        }

        /// <summary>
        /// The accumulative cost of the path until now.
        /// </summary>
        public float Cost
        {
            get;
            set;

        }

        /// <summary>
        /// The estimated cost to the goal from here. It tries to estimate taking into consideration obstacles
        /// on the way.
        /// </summary>
        public float GoalEstimate
        {
            get
            {
                if (controller.GoalNode.Equals(this))
                    return 0;

                Vector2 from = MeshNode.Polygon.Centroid;
                return controller.PathHeuristic.EstimatePathCost(from, controller.GoalPosition);
            }
        }

        /// <summary>
        /// The cost plus the estimated cost to the goal from here.
        /// </summary>
        public float TotalCost
        {
            get
            {
                return Cost + GoalEstimate;
            }
        }
        #endregion

        #region Constructors

        /// <summary>
        /// Constructor.
        /// </summary>
        /// <param name="controller">Allow refernce to finder so nodes can access information about goal position and other properties.</param>
        /// <param name="parent">Store node's parent in order to rebuild path.</param>
        /// <param name="meshNode">Underlying mesh node with map information</param>
        /// <param name="cost">The accumulative cost until now</param>
        public AStarNode(AStar controller, AStarNode parent, MeshNode meshNode, float cost)
        {
            Parent = parent;
            MeshNode = meshNode;
            Cost = cost;
            this.controller = controller;
        }
        #endregion

        #region Public Methods

        /// <summary>
        /// Gets all "feasible" successors nodes from the current node and adds them to the successor list.
        /// This method might present diferent neighboors according to environment. 
        /// </summary>
        /// <remarks>TODO: Currently, dynamic entities are not taken into account.</remarks>
        /// <param name="successors">List in which the successors will be added</param>
        public void GetSuccessors(List<AStarNode> successors)
        {
            for (int i = 0; i < MeshNode.Neighbors.Count; i++)
            {
                MeshNode neighboor = MeshNode.Neighbors[i];
                if (Parent != null && neighboor.Equals(Parent.MeshNode))
                    continue;
                
                //use distance from cetroid of current mesh to midpoint in the boundary, except if at current where
                //we use the actual original position of entity
                Vector2 fromPosition = MeshNode.Equals(controller.StartNode) ? controller.StartPosition : MeshNode.Polygon.Centroid;

                //we can use midpoint or centroid Boundaries[i] MidPoint
                float currentCost = Vector2.Distance(fromPosition, MeshNode.Boundaries[i].MidPoint);

                AStarNode newNode = new AStarNode(controller, this, neighboor, Cost + currentCost);

                successors.Add(newNode);
            }
        }

        #endregion

        #region Overridden Methods

        public override bool Equals(object obj)
        {
            if(obj is AStarNode)
            {
                return MeshNode.Equals(((AStarNode)obj).MeshNode);    
            }
            return false;
        }

        public override int GetHashCode()
        {
            return base.GetHashCode();
        }

        #endregion

        #region IComparable Members

        public int CompareTo(object obj)
        {
            return (-TotalCost.CompareTo(((AStarNode)obj).TotalCost));
        }

        #endregion
    }



    /// <summary>
    /// Class for performing A* pathfinding
    /// </summary>
    sealed class AStar
    {
        #region Properties
        public Vector2 StartPosition
        {
            get; private set;
        }

        public Vector2 GoalPosition
        {
            get; private set;
        }
        public AStarNode GoalNode
        {
            get; private set;
        }
        public AStarNode StartNode
        {
            get; private set;
        }

        public World World
        {
            get; private set;
        }

        public IPathHeuristic PathHeuristic
        {
            get; private set;
        }
        #endregion

        #region Constructors
        /// <summary>
        /// Instantiate A* algorithm with given heuristic to estimate cost between two points.
        /// </summary>
        /// <param name="pathHeuristic"></param>
        public AStar(IPathHeuristic pathHeuristic)
        {
            if (pathHeuristic == null) throw new ArgumentNullException("pathHeuristics");
            PathHeuristic = pathHeuristic;
        }
        #endregion

        #region Public Methods

        /// <summary>
        /// Finds the shortest path from the start node to the goal node
        /// </summary>
        /// <param name="startPosition">Start position</param>
        /// <param name="goalPosition">Goal position</param>
        /// <param name="startMeshNode">Start mesh node</param>
        /// <param name="goalMeshNode">Goal mesh node</param>
        public List<MeshNode> FindPath(Vector2 startPosition, Vector2 goalPosition, MeshNode startMeshNode, MeshNode goalMeshNode)
        {
            StartPosition = startPosition;
            GoalPosition = goalPosition;

            GoalNode = new AStarNode(this, null, goalMeshNode, 0);
            StartNode = new AStarNode(this, null, startMeshNode, 0);

            var openList = new Heap();
            var closedList = new Heap();
            var solution = new List<AStarNode>();
            var successors = new List<AStarNode>();

            openList.Add(StartNode);

            while (openList.Count > 0)
            {
                // Get the node with the lowest TotalSum
                var nodeCurrent = (AStarNode)openList.Pop();

                // If the node is the goal copy the path to the solution array
                if (GoalNode.Equals(nodeCurrent))
                {
                    while (nodeCurrent != null)
                    {
                        solution.Insert(0, nodeCurrent);
                        nodeCurrent = nodeCurrent.Parent;
                    }
                    
                    //convert solution 
					List<MeshNode> meshNodes = new List<MeshNode>();

					for (int i = 0; i < solution.Count; i++)
					{
						meshNodes.Add(solution[i].MeshNode);
					}

					return meshNodes;
                    //return solution.ConvertAll(an => an.MeshNode);
                }

                // Get successors to the current node
                successors.Clear();
                nodeCurrent.GetSuccessors(successors);
                foreach (var nodeSuccessor in successors)
                {
                    // Test if the currect successor node is on the open list, if it is and
                    // the TotalSum is higher, we will throw away the current successor.
                    AStarNode nodeOpen;
                    if (openList.Contains(nodeSuccessor))
                    {
                        nodeOpen = (AStarNode)openList[openList.IndexOf(nodeSuccessor)];
                        if ((nodeOpen != null) && (nodeSuccessor.TotalCost > nodeOpen.TotalCost))
                            continue;
                    }

                    // Test if the currect successor node is on the closed list, if it is and
                    // the TotalSum is higher, we will throw away the current successor.
                    AStarNode nodeClosed;
                    if (closedList.Contains(nodeSuccessor))
                    {
                        nodeClosed = (AStarNode)closedList[closedList.IndexOf(nodeSuccessor)];
                        if ((nodeClosed != null) && (nodeSuccessor.TotalCost > nodeClosed.TotalCost))
                            continue;
                    }

                    // Remove the old successor from the open list
                    openList.Remove(nodeSuccessor);

                    // Remove the old successor from the closed list
                    closedList.Remove(nodeSuccessor);

                    // Add the current successor to the open list
                    openList.Push(nodeSuccessor);
                }
                // Add the current node to the closed list
                closedList.Add(nodeCurrent);
            }
            
            return null;
        }

        #endregion
    }
}